#!/usr/bin/python
## NAO-ROBOTICS FINAL PROJECT 
'''
  CS494 FALL 2011 
  DR. PARKER
  GROUP 3: 
    TYLER BURKLE
    TRUSHNA PATEL
    CURTIS TAYLOR
'''
# TO DO : 
# Robot needs to stop better. 

from naoqi import *
from math import pi
import sys
import time

def cleanShutDown(track,red,b,speech,mp):
  #NEED a stop all action before trying to sit down
  track.stopTracker()
  #Unsubscribe ALRedBallDetection
  red.unsubscribe("redBallDetected")
  b.stopAllBehaviors()
  #Once robot is done collecting red balls, make it sit down
  speech.post.say("Sitting down")
  b.runBehavior("sitDown")
  mp.setStiffnesses('Body', 0)
  sys.exit()
  
def findBall(video,track,mp,red,mem,speech,b,init):
  
  turnLeft = [init[0],init[1],init[2],init[3],init[4],0.7164405584335327] 
  turnRight = [init[0],init[1],init[2],init[3],init[4],-0.7164405584335327] 
  turnUp = [init[0],init[1],init[2],init[3],-0.748485267162323,init[5]]
  turnDown = [init[0],init[1],init[2],init[3],0.22998255491256714,init[5]]
  #Turn Left
  #mp.setPosition("Head",2,turnLeft,0.2,63)
  mp.setPosition("Head",2,turnDown,0.4,63)
  print "TurnDown \n : ", mp.getPosition("Head",2,True)
  if track.isNewData(): 
    return True
  headMiddle = mem.post.getData('Device/SubDeviceList/Head/Touch/Middle/Sensor/Value', 0)
  if headMiddle != 0 : cleanShutDown(track,red,b,speech,mp)
  time.sleep(4)
  mp.setPosition("Head",2,init,0.4,63)
  print "Init \n : " , mp.getPosition("Head",2,True)
  if track.isNewData(): 
    return True
  headMiddle = mem.post.getData('Device/SubDeviceList/Head/Touch/Middle/Sensor/Value', 0)
  if headMiddle != 0 : cleanShutDown(track,red,b,speech,mp)
  time.sleep(4)
  mp.setPosition("Head",2,turnLeft,0.4,63)
  print "TurnLeft \n : ", mp.getPosition("Head",2,True)
  if track.isNewData(): 
    return True
  headMiddle = mem.post.getData('Device/SubDeviceList/Head/Touch/Middle/Sensor/Value', 0)
  if headMiddle != 0 : cleanShutDown(track,red,b,speech,mp)
  time.sleep(4)
  mp.setPosition("Head",2,init,0.4,63)
  print "Init \n : " , mp.getPosition("Head",2,True)
  if track.isNewData(): 
    return True
  headMiddle = mem.post.getData('Device/SubDeviceList/Head/Touch/Middle/Sensor/Value', 0)
  if headMiddle != 0 : cleanShutDown(track,red,b,speech,mp)
  time.sleep(4)
  mp.setPosition("Head",2,turnUp,0.4,63)
  print "TurnUp \n : " , mp.getPosition("Head",2,True)
  if track.isNewData(): 
    return True
  headMiddle = mem.post.getData('Device/SubDeviceList/Head/Touch/Middle/Sensor/Value', 0)
  if headMiddle != 0 : cleanShutDown(track,red,b,speech,mp)
  time.sleep(4)
  mp.setPosition("Head",2,init,0.4,63)
  print "Init \n : " , mp.getPosition("Head",2,True)
  if track.isNewData(): 
    return True
  headMiddle = mem.post.getData('Device/SubDeviceList/Head/Touch/Middle/Sensor/Value', 0)
  if headMiddle != 0 : cleanShutDown(track,red,b,speech,mp)
  time.sleep(4)
  mp.setPosition("Head",2,turnRight,0.4,63)
  print "TurnRight \n : " , mp.getPosition("Head",2,True)
  if track.isNewData(): 
    return True
  headMiddle = mem.post.getData('Device/SubDeviceList/Head/Touch/Middle/Sensor/Value', 0)
  if headMiddle != 0 : cleanShutDown(track,red,b,speech,mp)
  time.sleep(4)
  mp.setPosition("Head",2,init,0.4,63)
  print "Init \n : " , mp.getPosition("Head",2,True)
  if track.isNewData(): 
    return True
  headMiddle = mem.post.getData('Device/SubDeviceList/Head/Touch/Middle/Sensor/Value', 0)
  if headMiddle != 0 : cleanShutDown(track,red,b,speech,mp)
  time.sleep(4)

  return False
'''
  
  
        mp.angleInterpolation(names,right,time,True)  
        headMiddle = mem.getData('Device/SubDeviceList/Head/Touch/Middle/Sensor/Value', 0)
        if headMiddle != 0 : cleanShutDown(track,red,b,speech,mp)
        if track.isNewData(): 
          return True
  return False
'''

#IP = '10.26.210.61' #robot's IP jazz
#IP = '10.26.210.60' #robot's IP IronHide
IP = '10.26.210.59' #robot's IP - bumblebee
PORT = 9559
speech  = ALProxy("ALTextToSpeech", IP, PORT) 
mp  = ALProxy("ALMotion", IP, PORT) 
b  = ALProxy("ALBehaviorManager", IP, PORT) 
sonar  = ALProxy("ALSonar", IP, PORT) 
mem  = ALProxy("ALMemory", IP, PORT)
red  = ALProxy("ALRedBallDetection", IP, PORT)
track  = ALProxy("ALRedBallTracker", IP, PORT)
video   = ALProxy("ALVideoDevice", IP, PORT)

video.kCameraSelectID = 18
#print video.getParam(video.kCameraSelectID)
sonar.subscribe("SonarTest", 500, 1.0) 

period = 500

#Stand up
speech.post.say("Standing Up") 
mp.setStiffnesses('Body', 1)
b.runBehavior('standUp')
mp.setWalkArmsEnable(True,True)

#Stop robot if head is tapped
headMiddle = 0
memValue = 'redBallDetected'
red.subscribe("redBallDetected", period, 0.0)
speech.post.say("Start Tracking")
#track.startTracker()

video.setParam(video.kCameraSelectID, 1)

ballFound = False
init = mp.getPosition("Head",2,True)

while headMiddle == 0:
  #should robot be turning while below loop is running?
  if ballFound == False :
    ballFound = findBall(video,track,mp,red,mem,speech,b,init)
      
  headMiddle = mem.getData('Device/SubDeviceList/Head/Touch/Middle/Sensor/Value', 0)
  if headMiddle != 0 : cleanShutDown(track,red,b,speech,mp)
  
  val = mem.getData(memValue, 0)  
  pos_array = track.getPosition()
  #print pos_array

  x = pos_array[0]
  y = pos_array[1]
  z = pos_array[2]
  headMiddle = mem.getData('Device/SubDeviceList/Head/Touch/Middle/Sensor/Value', 0)
  if headMiddle != 0 : cleanShutDown(track,red,b,speech,mp)


cleanShutDown(track,red,b,speech,mp)

